#include "Kinematics.h"

Kinematics::Kinematics(void)
{
}

Kinematics::Kinematics(float d0, float d1, float a2, float d4)
{
	this->d0 = d0;
	this->d1 = d1;
	this->a2 = a2;
	this->d4 = d4;
}

Kinematics::~Kinematics(void)
{
}

vector<float> Kinematics::error(std::string message){
	std::cout << message;
	vector<float> joints(3,0);
	return joints;
}

vector<float> Kinematics::inverse(Point p)
{
	float w, stetaw, ctetaw, tetaw, stetak, tetak, teta1;
	float ctetak, steta2, cteta2, teta2, cteta3, steta3, teta3;
	string message;
	vector<float> joints;

	
	//printf("p.x: %f \n", p.getValue(Point::X));
	//printf("p.y: %f \n", p.getValue(Point::Y));
	//printf("p.z: %f \n", p.getValue(Point::Z));
	

	Point o = Point(d1, 0, d0);
	
	//printf("o.x: %f \n", o.getValue(Point::X));
	//printf("o.y: %f \n", o.getValue(Point::Y));
	//printf("o.z: %f \n", o.getValue(Point::Z));
		

	w = p.distance(o);
	//printf("w: %f \n", w);
	stetaw = p.getValue(Point::Z)-d0;
	ctetaw = p.getValue(Point::Y);
	if((stetaw == 0) && (ctetaw == 0)){
		//message = "Errore tetaw: Punto non raggiungibile\n";
		tetaw = 0.f;
	}
	else{
		tetaw = atan2(stetaw, ctetaw) + (M_PI/2);
	}
		ctetak = (pow(d4, 2) - pow(w, 2) - pow(a2, 2))/(-2*w*a2);
		if(pow(ctetak, 2) > 1){
			//message = "Errore stetak: Punto non raggiungibile\n";
			ctetak = 1.f;
		}
		else{
			stetak = sqrt(1-pow(ctetak, 2));
		}
			if((stetak == 0) && (ctetak == 0)){
				message = "Errore teta1: Punto non raggiungibile\n";
				joints = error(message); 
			}
			else{
				tetak = atan2(stetak, ctetak);
				teta1 = tetaw - tetak;
			//	printf("teta1: %f \n", teta1);
				steta2 = p.getValue(Point::X)-d1;
				cteta2 = sqrt(pow(p.getValue(Point::Z)-d0, 2) + pow(p.getValue(Point::Y), 2));
				if((steta2 == 0) && (cteta2 == 0)){
					message = "Errore teta2: Punto non raggiungibile\n";
					joints = error(message);
				}
				else{
					teta2 = atan2(steta2, cteta2);
				//	printf("teta2: %f \n", teta2);
					cteta3 = ((w*ctetak) - a2)/d4;
					if(pow(cteta3, 2) > 1){
						message = "Errore steta3: Punto non raggiungibile\n";
						joints = error(message);
					}
					else{
						steta3 = sqrt(1- pow(cteta3, 2));
						if((steta3 == 0) && (cteta3 == 0)){
							message = "Errore teta3: Punto non raggiungibile\n";
							joints = error(message);
						}
						else{
							teta3 = atan2(steta3, cteta3);
						//	printf("teta3: %f \n", teta3);
							joints.push_back(teta1);
							joints.push_back(teta2);
							joints.push_back(teta3);
						//	printf("teta1: %f, teta2: %f, teta3: %f\n", joints[0], joints[1], joints[2]);
						}
					}
				}
			}
		//}
	//}
	return joints;
}